import launch
from launch.substitutions import Command, LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from nav2_common.launch import RewrittenYaml
import launch_ros
import os
from launch.actions import TimerAction

def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='rm_bringup').find('rm_bringup')
    bringup_dir = get_package_share_directory('nav2_bringup')
    slam_toolbox_dir = get_package_share_directory('slam_toolbox')
    default_model_path = os.path.join(pkg_share, 'src/description/new.urdf')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz/nav2_default_view.rviz')
    world_path=os.path.join(pkg_share, 'world/2023_7_28.sdf')
    slam_yaml_path=os.path.join(pkg_share, 'config/slam_params.yaml')
    slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'online_async_launch.py')
    nav2_params='/home/ikun/nav2_ws/src/rm_bringup/config/nav2_params_gazebo.yaml'
    launch_dir = os.path.join(bringup_dir, 'launch')
    param_substitutions = {
        'use_sim_time': LaunchConfiguration('use_sim_time'),
        'yaml_filename': '/home/ikun/nav2_ws/my_map.yaml'}
    configured_params = RewrittenYaml(
            source_file=nav2_params,
            param_rewrites=param_substitutions,
            convert_types=True)

    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
    )
    
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher'
    )
    # joint_state_publisher_gui_node = launch_ros.actions.Node(
    #     package='joint_state_publisher_gui',
    #     executable='joint_state_publisher_gui',
    #     name='joint_state_publisher_gui',
        
    # )

    rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen'
    )

    # 机器人初始位置
    spawn_entity = launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'sam_bot', '-topic', 'robot_description','-x','2.5','-y','0.0','-z','0.5'],#3v3
        # arguments=['-entity', 'sam_bot', '-topic', 'robot_description','-x','7','-y','5','-z','1.0'],#7v7
        output='screen'   
    )

    #点云降维--切片法
    point_clout_convert =launch_ros.actions.Node(
        package="pointcloud_to_laserscan",
        executable="pointcloud_to_laserscan_node",
        remappings=[('/cloud_in', '/livox/lidar'),],
        parameters=[{"min_height":0.1},
                    {"max_height":0.55},
                    {"scan_time":0.1},
                    {"range_min":0.43},
                    {"range_max":14.0},
                    {"angle_increment":3.14159/900},
                    {"target_frame":"livox_frame"}] # 0.2deg
    )

    #点云降维--梯度法
    point_clound_slicing =launch_ros.actions.Node(
        package="point_clound_slicing",
        executable="point_clound_slicing",
        output='screen'
    )

    # 订阅/scan_out，进行聚类分析，输出机器人的方位
    robot_cluster=launch_ros.actions.Node(
        package="robot_cluster",
        executable="robot_cluster",
        output='screen'
    )

    # 订阅雷达和地图，滤除点云中和地图重合的点，保留机器人的点，并把这些点位发布到/scan_out
    scanmap_filter=launch_ros.actions.Node(
        package="scanmap_filter",
        executable="scanmap_filterpy",
        output='screen'
    )

    start_map_saver_server_cmd = launch_ros.actions.Node(
            package='nav2_map_server',
            executable='map_saver_server',
            output='screen',
            parameters=[configured_params])
    
    lifecycle_nodes = ['map_server']

    start_lifecycle_manager_cmd = launch_ros.actions.Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_localization',
            output='screen',
            arguments=['--ros-args', '--log-level', 'info'],
            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')},
                        {'autostart': True},
                        {'node_names': lifecycle_nodes}])

    start_map_server_cmd = launch_ros.actions.Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            # respawn='False',
            # respawn_delay=2.0,
            parameters=[configured_params],
            arguments=['--ros-args', '--log-level', 'info'],
            remappings=[('/tf', 'tf'),
                  ('/tf_static', 'tf_static')
                  ])

    start_amcl_cmd=launch_ros.actions.Node(
            package='nav2_amcl',
            executable='amcl',
            name='amcl',
            output='screen',
            parameters=[configured_params],
            arguments=['--ros-args', '--log-level', 'info'],
            remappings=[('/tf', 'tf'),('/tf_static', 'tf_static')]
    )

    # 卡尔曼滤波
    robot_localization_node = launch_ros.actions.Node(
       package='robot_localization',
       executable='ekf_node',
       name='ekf_filter_node',
       output='screen',
       parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': LaunchConfiguration('use_sim_time')}]
    )

    start_slam_toolbox_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(slam_launch_file),
        launch_arguments={'slam_params_file': LaunchConfiguration('slam_params')}.items(),)
    

    start_localization_cmd=IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(launch_dir,'localization_launch.py')),
            launch_arguments={'map': os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'),
                              'params_file': LaunchConfiguration('nav2_params'),
                              'container_name': 'nav2_container'}.items())

    nav2_cmd=IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
        launch_arguments={'params_file': LaunchConfiguration('nav2_params'),
                          'container_name': 'nav2_container'}.items())
    
    slam_gmapping_cmd=IncludeLaunchDescription(
        PythonLaunchDescriptionSource('/home/ikun/nav2_ws/src/slam_gmapping-eloquent-devel/slam_gmapping/launch/slam_gmapping.launch.py'))

    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
                                            description='Absolute path to robot urdf file'),
        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
                                            description='Absolute path to rviz config file'),
        launch.actions.DeclareLaunchArgument(name='world', default_value=world_path,
                                            description='Absolute path to world config file'),
        launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='true',
                                            description='Flag to enable use_sim_time'),
        launch.actions.DeclareLaunchArgument(name='slam_params', default_value=slam_yaml_path,
                                            description='Absolute path to slam file'),
        launch.actions.DeclareLaunchArgument(name='nav2_params', default_value=nav2_params,
                                            description='Absolute path to nav2 file'),

        # 仿真UI：gazebo和rviz
        launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', 
                                           '-s', 'libgazebo_ros_factory.so',
                                           LaunchConfiguration('world')], 
                                           output='screen'),
        rviz_node,

        # 仿真启动
        robot_state_publisher_node,
        joint_state_publisher_node,
        spawn_entity,

        # 三维点云转二维雷达
        # point_clout_convert,
        # TimerAction(period=6.0, actions=[point_clound_slicing]),

        # slam导航
        slam_gmapping_cmd,

        # 导航
        nav2_cmd        
    ])
# 命令：
# ros2 bag play rmuc_0603-173817_0
# export MESA_D3D12_DEFAULT_ADAPTER_NAME="NVIDIA"
# ros2 launch rm_bringup display.launch.py model:='/mnt/e/nav2_ws/src/rm_bringup/src/description/Mwheel_3D.urdf' nav2_params:='/mnt/e/nav2_ws/src/rm_bringup/config/nav2_params.yaml' rvizconfig:='/mnt/e/nav2_ws/src/rm_bringup/rviz/nav2_default_view.rviz' world:='/mnt/e/nav2_ws/src/rm_bringup/world/2023_v_4_1.sdf'
# cmake:add_executable + ament_target_dependencies + install添加
# ros2 launch rm_bringup display.launch.py world:='/home/ikun/nav2_ws/src/rm_bringup/world/2023_7_28.sdf' model:='/home/ikun/nav2_ws/src/rm_bringup/src/description/Mwheel_3D.urdf'